from time import sleep
from havi import ports

# --- Setup ---
servo = ports.servo1()                          # Servo on Servo1 port
ultrasonic_front = ports.input1("ultrasonic sensor")  # HC-SR04 on Input1
ultrasonic_back  = ports.input2("ultrasonic sensor")  # HC-SR04 on Input2

# Sweep parameters
min_angle = 0
max_angle = 180
step = 2       # degrees per step (smaller = smoother, but slower)
delay = 0.05   # delay between moves

while True:
    # Sweep left to right
    for angle in range(min_angle, max_angle + 1, step):
        servo.move(angle)
        sleep(delay)  # allow servo to move

        # --- Front sensor ---
        try:
            d1 = ultrasonic_front.distance_cm()
            print(f"{angle},{d1:.2f},front")
        except:
            print(f"{angle},OutOfRange,front")

        # --- Back sensor (mapped 180–360) ---
        back_angle = angle + 180
        try:
            d2 = ultrasonic_back.distance_cm()
            print(f"{back_angle},{d2:.2f},back")
        except:
            print(f"{back_angle},OutOfRange,back")

        sleep(0.1)

    # Sweep right to left
    for angle in range(max_angle, min_angle - 1, -step):
        servo.move(angle)
        sleep(delay)

        # --- Front sensor ---
        try:
            d1 = ultrasonic_front.distance_cm()
            print(f"{angle},{d1:.2f},front")
        except:
            print(f"{angle},OutOfRange,front")

        # --- Back sensor (mapped 180–360) ---
        back_angle = angle + 180
        try:
            d2 = ultrasonic_back.distance_cm()
            print(f"{back_angle},{d2:.2f},back")
        except:
            print(f"{back_angle},OutOfRange,back")

        sleep(0.1)

